clc
clear
load('middata.mat')
% 设置目标点
lon0 = 0;
lat0 = 82*pi/180;
%% guider
guider.V = sqrt(guider.v_gx.^2 + guider.v_gy.^2 + guider.v_gz.^2);
% 再入段末端状态量lon, lat, alt, v, fpa, azi
entryStatef1 = [guider.lon(end),guider.lat(end), guider.altitude(end), guider.V(end), guider.fpa_local(end), guider.azi_local(end)];
lla1 = [entryStatef1(1),entryStatef1(2),entryStatef1(3)];
XYZ1 = lla2ECEF(lla1);
ENU1 = XYZ2ENU(XYZ1, lat0, lon0);
state01 = [ENU1(1) ENU1(2) ENU1(3) entryStatef1(4) entryStatef1(5) entryStatef1(6)];
%% follower1
follower1.V = sqrt(follower1.v_gx.^2 + follower1.v_gy.^2 + follower1.v_gz.^2);
entryStatef2 = [follower1.lon(end),follower1.lat(end), follower1.altitude(end), follower1.V(end), follower1.fpa_local(end), follower1.azi_local(end)];
lla2 = [entryStatef2(1),entryStatef2(2),entryStatef2(3)];
XYZ2 = lla2ECEF(lla2);
ENU2 = XYZ2ENU(XYZ2, lat0, lon0);
state02 = [ENU2(1) ENU2(2) ENU2(3) entryStatef2(4) entryStatef2(5) entryStatef2(6)];
%% follower2
follower2.V = sqrt(follower2.v_gx.^2 + follower2.v_gy.^2 + follower2.v_gz.^2);
entryStatef3 = [follower2.lon(end),follower2.lat(end), follower2.altitude(end), follower2.V(end), follower2.fpa_local(end), follower2.azi_local(end)];
lla3 = [entryStatef3(1),entryStatef3(2),entryStatef3(3)];
XYZ3 = lla2ECEF(lla3);
ENU3 = XYZ2ENU(XYZ3, lat0, lon0);
state03 = [ENU3(1) ENU3(2) ENU3(3) entryStatef3(4) entryStatef3(5) entryStatef3(6)];

% 目标点 lat0 = 82*pi/180;
% state01 = [14908.9652253822,-103854.595474456,34256.7294873723,2719.92039498365,-0.00886516000000000,-0.113247000000000];
% state02 = [2945.75060559580,-103969.033356794,34776.2966363692,2716.52384974198,-0.00777919000000000,-0.101448000000000];
% state03 = [26891.2840907607,-104433.539876934,33856.2239223689,2719.64359356920,-0.00941172000000000,-0.125419000000000];
% state01 = [14908.9652253822,-215750.479519437,31467.5782526456,2719.92039498365,-0.00886516000000000,-0.113247000000000];
% state02 = [2945.75060559580,-215873.967669388,31985.0690526147,2716.52384974198,-0.00777919000000000,-0.101448000000000];
% state03 = [26891.2840907607,-216322.345959978,31057.0297135630,2719.64359356920,-0.00941172000000000,-0.125419000000000];